#include <string.h>
#include "maindefs.h"
#include "loconet_thread.h"
#include "messages.h"
#include "loconet.h"

/*
 * Initializes values in loconet_thread_struct
 */
void init_loconet_thread(loconet_thread_struct *lptr)
{
	lptr->msg_ready = 0;
	lptr->cur_len = 0;
	memset(lptr->cur_msg, 0, MSGLEN);
}

/*
 *  Returns 1 if the byte is an opcode, 0 if not
 */
unsigned char is_opcode(unsigned char byte) {
	// the most significant bit is the opcode flag
	return (byte >> 7);
}

/*
 *  Returns 1 if the checksum is valid, 0 if not.
 *  Note length is the full length of the message including the checksum
 */
unsigned char is_valid_checksum(unsigned char cksum, char *msg, unsigned char length) {
	unsigned char checker = msg[0];
	unsigned char i = 1;
	// XOR all the data bytes together
	for(; i < length-1; i++) {
		checker ^= msg[i];
	}
	// One's complement
	checker = ~checker;
	return (checker == cksum);
}

/*
 * Returns the size of a loconet message with the specified opcode.
 * The size is including the opcode and checksum.
 * Returns -1 if unrecognized opcode, 0 if variable length or the actual
 * length if a recognized opcode and set length.
 */
signed char message_size(unsigned char opc) {
	unsigned char msg_type = 0xF0 & opc;
	signed char retval = -1;
	switch(msg_type) {
	case 0x80:
		retval = 2;
		break;
	case 0xB0:
		retval = 4;
		break;
	case 0xA0:
		retval = 4;
		break;
	case 0xE0:
		retval = 0; // variable size message
		break;
	}
	return retval;
}	

/*
 * This is a "logical" thread that processes messages from LocoNet
 * It is not a "real" thread because there is only the single main thread
 * of execution on the PIC because we are not using an RTOS.
 */
int loconet_lthread(loconet_thread_struct *lptr, int msgtype, int length, unsigned char *msgbuffer)
{
	if (msgtype == MSGT_OVERRUN) {
		// If we overran the uart buffer do what?
	} else if (msgtype == MSGT_UART_DATA) {
		unsigned char i = 0;
		unsigned char cur_byte;
		for(;i<length;i++) {
			cur_byte = msgbuffer[i];
			// Iterate through all of the uart data, parsing out
			// loconet messages
			if(lptr->cur_len == 0) {
				// New loconet message?
				if(is_opcode(cur_byte) == 1) {
					// Start of a new message
					lptr->cur_len = 1;
					lptr->cur_msg[0] = cur_byte;
					lptr->msg_ready = 0;
				}
				else {
					// Invalid byte, may have been in the middle of a message
					// before we received the opcode move onto the next byte
					// Reset values
					lptr->cur_len = 0;
					lptr->msg_ready = 0;
					continue;
				}
			}
			else {
				// In the middle of a message
				signed char full_len = message_size(lptr->cur_msg[0]);

				if(full_len == 0) {
					// Variable length message
					if(lptr->cur_len == 1) {
						// At the length byte
						full_len = cur_byte;
					}
					else if(lptr->cur_len > 1) {
						// Length byte already stored, retrieve it
						full_len = lptr->cur_msg[1];
					}
				}
				else if(full_len < 0) {
					// Error, an incorrect opcode was provided
					HANDLE_ERROR(-5);
				}

				if(lptr->cur_len < full_len - 1) {
					// This byte is an argument, store it
					lptr->cur_msg[lptr->cur_len] = cur_byte;
					// Increment cur_len
					lptr->cur_len++;
				}
				else if(lptr->cur_len == full_len - 1) {
					// This byte is a checksum, check it
					unsigned char checksum = cur_byte;
					if(is_valid_checksum(checksum,lptr->cur_msg,full_len)) {
						// If the checksum is valid, we have the full message
						lptr->cur_msg[lptr->cur_len] = checksum;
						lptr->cur_len++;
						// Set msg_ready to 1
						lptr->msg_ready = 1;
					}
				}
				else {
					// Shouldn't get here
					HANDLE_ERROR(-6);
				}
			}

			// Check if a LocoNet message is ready to be sent to I2C
			if(lptr->msg_ready) {
				// Send current loconet message to I2C
				FromMainHigh_sendmsg(lptr->cur_len, MSGT_LOCONET_MSG, (void *) lptr->cur_msg);
				// Set msg_ready and cur_len back to 0
				lptr->msg_ready = 0;
				lptr->cur_len = 0;
			}
		}
	}
}
